#include "pch.h"
#include "OpencvPro.h"
#include <opencv2/opencv.hpp>
//#include "opencv2/video/tracking.hpp"
#include "CvTool.h"
#include <algorithm>

using namespace cv;

// char*转Mat
Mat Char2Mat(unsigned char* buffer, int itype, int w, int h)
{
	cv::Mat img;
	switch (itype)
	{
	case 1: {
		cv::Mat tp_img(h, w, CV_8UC1, buffer); img = tp_img;
		break; }
	case 3: {
		cv::Mat tp_img(h, w, CV_8UC1, buffer);
		break; }
	case 4: {
		cv::Mat tp_img(h, w, CV_8UC1, buffer); img = tp_img;
		break; }
	default:; break;
	}
	//cv::Mat grayImg(w*3,h, CV_8UC1);
	return img;
}

/* Mark点识别 */
int OpencvPro::RecognitionMark(unsigned char*buff, double*param, vector<float>*vx, vector<float>*vy, vector<float>*vr)
{
	Mat img = Char2Mat(buff, (int)param[0], (int)param[1], (int)param[2]);
	/*Mat img;
	resize(imgOrg, img, Size(int(imgOrg.cols*0.5),int(imgOrg.rows*0.5)));*/
	Mat scaleImg,threshImg,openImg,connImg,fillupImg;
	normalize(img, scaleImg,0.0,255.0, NORM_MINMAX);//归一化处理
	threshold(scaleImg, threshImg,128.0,255, THRESH_BINARY_INV);
	openImg = CvTool::OpeningCircle(threshImg,5);
	//找轮廓（相当与halcon的区域）
	vector<vector<Point>> contours;
	vector<Vec4i> hireachy;
	//二值图找轮廓：二值图；轮廓；关系；关系参数；点参数；略；
	findContours(openImg, contours, hireachy, RETR_CCOMP, CHAIN_APPROX_SIMPLE,Point());
	Mat resImg = CvTool::DrawContoursRGB(contours, openImg.size());
	vector<int> index;
	for (size_t t = 0; t < contours.size(); t++)
	{
		double len = arcLength(contours[t],true);//通过周长筛选掉大面积轮廓
		if (len>2000) continue;
		double area = contourArea(contours[t]);//面积大时很耗时
		Point2f center; float radius;
		minEnclosingCircle(contours[t], center, radius);
		double circularity = (area) / (CV_PI*radius*radius);//圆度
		if (area > 2000 && circularity > 0.9)
		{
			vx->push_back(center.x*2);
			vy->push_back(center.y*2);
			vr->push_back(radius*2);
			index.push_back(t);
		}
	}
	//去除同心子轮廓
	for (int i = 0;i< index.size();i++)
	{
		Vec4i v4i = hireachy[index[i]];
		int fcon = v4i[3];//父序号
		//查找父项，存在则移除自身
		vector<int>::iterator it = find(index.begin(), index.end(), fcon);
		if (it != index.end()) //存在父项
		{
			vx->erase(vx->begin()+i);
			vy->erase(vy->begin() + i);
			vr->erase(vr->begin() + i);
			index.erase(it);
		}
	}
	return 0;
}

Mat LocalAffineEstimate(const vector<Point2f>& shape1, const vector<Point2f>& shape2,bool fullAfine)
{
	Mat out(2, 3, CV_32F);
	int siz = 2 * (int)shape1.size();

	if (fullAfine)
	{
		Mat matM(siz, 6, CV_32F);
		Mat matP(siz, 1, CV_32F);
		int contPt = 0;
		for (int ii = 0; ii < siz; ii++)
		{
			Mat therow = Mat::zeros(1, 6, CV_32F);
			if (ii % 2 == 0)
			{
				therow.at<float>(0, 0) = shape1[contPt].x;
				therow.at<float>(0, 1) = shape1[contPt].y;
				therow.at<float>(0, 2) = 1;
				therow.row(0).copyTo(matM.row(ii));
				matP.at<float>(ii, 0) = shape2[contPt].x;
			}
			else
			{
				therow.at<float>(0, 3) = shape1[contPt].x;
				therow.at<float>(0, 4) = shape1[contPt].y;
				therow.at<float>(0, 5) = 1;
				therow.row(0).copyTo(matM.row(ii));
				matP.at<float>(ii, 0) = shape2[contPt].y;
				contPt++;
			}
		}
		Mat sol;
		solve(matM, matP, sol, DECOMP_SVD);
		out = sol.reshape(0, 2);
	}
	else
	{
		Mat matM(siz, 4, CV_32F);
		Mat matP(siz, 1, CV_32F);
		int contPt = 0;
		for (int ii = 0; ii < siz; ii++)
		{
			Mat therow = Mat::zeros(1, 4, CV_32F);
			if (ii % 2 == 0)
			{
				therow.at<float>(0, 0) = shape1[contPt].x;
				therow.at<float>(0, 1) = shape1[contPt].y;
				therow.at<float>(0, 2) = 1;
				therow.row(0).copyTo(matM.row(ii));
				matP.at<float>(ii, 0) = shape2[contPt].x;
			}
			else
			{
				therow.at<float>(0, 0) = -shape1[contPt].y;
				therow.at<float>(0, 1) = shape1[contPt].x;
				therow.at<float>(0, 3) = 1;
				therow.row(0).copyTo(matM.row(ii));
				matP.at<float>(ii, 0) = shape2[contPt].y;
				contPt++;
			}
		}
		Mat sol;
		solve(matM, matP, sol, DECOMP_SVD);
		out.at<float>(0, 0) = sol.at<float>(0, 0);
		out.at<float>(0, 1) = sol.at<float>(1, 0);
		out.at<float>(0, 2) = sol.at<float>(2, 0);
		out.at<float>(1, 0) = -sol.at<float>(1, 0);
		out.at<float>(1, 1) = sol.at<float>(0, 0);
		out.at<float>(1, 2) = sol.at<float>(3, 0);
	}
	return out;
}

/* 
* N点标定(含旋转标定)
* count:[0]平移点数[1]旋转点数
*/
void OpencvPro::NPointCliab(
	float*camPx, float*camPy, //相机
	float*robotPx, float*robotPy, //机器人
	float*rotatePx, float*rotatePy,//旋转像素点（最后一个是物理旋转中心）
	int* count, float*oMatrix)
{
	vector<Point2f>camPt, robotPt;
	for (int i = 0;i< count[0];i++)
	{
		camPt.push_back(Point2f(camPx[i], camPy[i]));
		robotPt.push_back(Point2f(robotPx[i], robotPy[i]));
	}
	Mat affine;
	estimateRigidTransform(camPt, robotPt,true).convertTo(affine,CV_32F);
	if (affine.empty())
	{
		affine = LocalAffineEstimate(camPt, robotPt, true);
	}
	float A, B, C, D, E, F;
	oMatrix[0] = A = affine.at<float>(0, 0);
	oMatrix[1] = B = affine.at<float>(0, 1);
	oMatrix[2] = C = affine.at<float>(0, 2);
	oMatrix[3] = D = affine.at<float>(1, 0);
	oMatrix[4] = E = affine.at<float>(1, 1);
	oMatrix[5] = F = affine.at<float>(1, 2);
	oMatrix[7] = oMatrix[6] = 0;
	oMatrix[8] = 1;
	//RMS 标定偏差
	std::vector<Point2f> points_Calc;
	double sumX = 0, sumY = 0;
	for (int i = 0; i < camPt.size(); i++)
	{
		Point2f pt;
		pt.x = A * camPt[i].x + B * camPt[i].y + C;
		pt.y = D * camPt[i].x + E * camPt[i].y + F;
		points_Calc.push_back(pt);
		sumX += pow(robotPt[i].x - points_Calc[i].x, 2);
		sumY += pow(robotPt[i].y - points_Calc[i].y, 2);
	}
	double rmsX, rmsY;
	rmsX = sqrt(sumX / camPt.size());
	rmsY = sqrt(sumY / camPt.size());
	//旋转标定(旋转点数只能为3)
	if (count[1] != 3) return;
	vector<Point2f>rotatePt;
	for (int i = 0; i < count[1]; i++)
		rotatePt.push_back(Point2f(rotatePx[i], rotatePy[i]));
	Point2f cent = CvTool::FitCircle3Point(rotatePt[0], rotatePt[1], rotatePt[2]);
	float centWx = A * cent.x + B * cent.y + C;
	float centWy = D * cent.x + E * cent.y + F;
	float dtx = rotatePx[count[1]] - centWx;
	float dty = rotatePy[count[1]] - centWy;
	Mat affineMatrix = (cv::Mat_<float>(3, 3) << A, B, C, D, E, F,0,0,1);
	Mat translationMatrix = (cv::Mat_<float>(3, 3) << 1, 0, dtx, 0, 1, dty,0,0,1);
	Mat rotateMatrix = translationMatrix * affineMatrix;
	oMatrix[0] = rotateMatrix.at<float>(0, 0);
	oMatrix[1] = rotateMatrix.at<float>(0, 1);
	oMatrix[2] = rotateMatrix.at<float>(0, 2);
	oMatrix[3] = rotateMatrix.at<float>(1, 0);
	oMatrix[4] = rotateMatrix.at<float>(1, 1);
	oMatrix[5] = rotateMatrix.at<float>(1, 2);
	oMatrix[6] = rotateMatrix.at<float>(2, 0);
	oMatrix[7] = rotateMatrix.at<float>(2, 1);
	oMatrix[8] = rotateMatrix.at<float>(2, 2);
}

